// Copyright (c) 2024 刻BITTER
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at http://mozilla.org/MPL/2.0/.


#pragma once

#include <cstddef>
#include <cstdint>

#include "ioxx_data_basic.hpp"

// DEBUG
// #include <iostream>

// DEBUG
// #include <cstdio>


#ifndef assert
    #ifndef NDEBUG
        #define assert(condition, when_fail) \
            do {                             \
                if (!(condition)) {          \
                    return;                  \
                }                            \
            } while (0)
    #else
        #define assert(condition, when_fail)
    #endif
#endif


// lipid <= just a short word has 'pid' in it
namespace lipid {
    /**
     * @brief 等比例映射
     *
     * 如果原数据超过了设置的原值域，那么映射后的数据也将超出新值域，映射函数本身不做限幅处理。
     *
     * @tparam V 数据类型
     * @param x 待映射的原始数据
     * @param in_min 原值域下界
     * @param in_max 原值域上界
     * @param out_min 新值域下界
     * @param out_max 新值域上界
     * @return V 映射后的新值
     */
    template <typename V>
    constexpr V value_map(V x, V in_min, V in_max, V out_min, V out_max) {
        return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
    }

    template <typename V>
    constexpr V value_constrain(V x, V min, V max) {
        if (x < min) {
            return min;
        }
        else if (x > max) {
            return max;
        }
        else {
            return x;
        }
    }


    template <typename V>
    class ValueClamp {
       protected:
        V _limit_min = 0;
        V _limit_max = 0;
        V _mapping_min = 0;
        V _mapping_max = 0;

        V _k = 0;
        V _b = 0;

        bool _use_mapping = false;
        bool _use_limit = false;

       public:
        ValueClamp(V limit_min, V limit_max) :
            _limit_min(limit_min), _limit_max(limit_max) {}

        ValueClamp(V limit_min, V limit_max, V mapping_min, V mapping_max) :
            _limit_min(limit_min),
            _limit_max(limit_max),
            _mapping_min(mapping_min),
            _mapping_max(mapping_max) {}

        ValueClamp() = default;

        V get_limit_min() const {
            return _limit_min;
        }

        V get_limit_max() const {
            return _limit_max;
        }

        void set_limit(V min, V max) {
            _limit_max = max;
            _limit_min = min;
        }

        void enable_mapping() {
            _use_mapping = true;
        }

        void disable_mapping() {
            _use_mapping = false;
        }

        bool is_mapping_effective() {
            return _use_mapping;
        }

        void enable_limit() {
            _use_limit = true;
        }

        void disable_limit() {
            _use_limit = false;
        }

        bool is_limit_effective() {
            return _use_limit;
        }

        V count(V value) {
            if (this->_use_limit)
                value = value_constrain(value, _limit_min, _limit_max);

            if (this->_use_mapping)
                value = value * _k + _b;

            return value;
        }

        V get_mapping_min() const {
            return _mapping_min;
        }


        V get_mapping_max() const {
            return _mapping_max;
        }


        void set_mapping(V min, V max) {
            _mapping_min = min;
            _mapping_max = max;

            _k = (_mapping_max - _mapping_min) / (_limit_max - _limit_min);
            _b = _mapping_min - _limit_min * _k;
        }
    };

    using DefaultFilterTimeType = uint32_t;
    constexpr size_t DefaultFilterBufferSize = 4;

    /**
     * @brief 滤波器接口
     *
     * @tparam V  输入数据类型
     * @tparam R  返回数据类型
     * @tparam T  时间戳类型
     */
    template <typename V, typename R, size_t N>
    class IFilter {
       public:
        static constexpr size_t BufferSize = N;
        /**
         * @brief 向滤波器输入一个值，并附带时间戳
         *
         * 对于不需要积累多个数据再输出的滤波器，feed 立即返回有效的滤波结果，
         * 反之，必须持续调用feed 输入数据，直到refreshed 返回true，表示滤波器输出了一个有效值，
         * 并且内部缓冲区已刷新。
         *
         * 某些滤波器可能需要输入的原始数据具有时间信息，此时要使用带有timestamp 参数的重载。
         *
         * @param val        待滤波的一个原始数据点
         * @param timestamp  时间戳
         * @return float
         */
        virtual R feed(V val) = 0;

        // virtual R feed(V val, T timestamp) = 0;

        virtual void reset() = 0;

        virtual ~IFilter() = default;
    };


    template <typename V, typename R, size_t N = DefaultFilterBufferSize>
    class MovingAverageFilter : public IFilter<V, R, N> {
       public:
        using WindowSizeType = data_basic::enough_index_type_t<N>;

       private:
        data_basic::RingBuffer<V, N> _buf;
        V _max_increment;  // 新加入的数据点相比前一个数据点的增量的最大值，
        // 如果超限，新数据视为无效，用前一数据点占位. 值为0 表示不设限.
        V _sum;
        V _init_avg;
        WindowSizeType _window_size;


       public:
        explicit MovingAverageFilter(V init_avg = 0, WindowSizeType window_size = N, V max_increment = 0) {
            this->init(init_avg, window_size, max_increment);
            this->reset();
        }

        virtual R feed(V val) override {
            if (_max_increment > 0) {
                V pre = _buf.back();
                V delta = std::abs(val - pre);

                if (delta > _max_increment) {  // 新数据超限，丢弃新数据，把前一个数据作为代替加入缓冲区
                    val = pre;
                }
            }

            _sum -= _buf.pop_front();
            _sum += val;
            _buf.push_back(val);
            return static_cast<R>(_sum / static_cast<V>(_window_size));
        }

        virtual void reset() override {
            _sum = _init_avg * static_cast<V>(_window_size);
            _buf.clear();
            _buf.init(_init_avg, _window_size);
        }

        void init(V init_avg, WindowSizeType window_size = N, V max_increment = 0) {
            assert(window_size <= N, "");
            _window_size = window_size;
            _init_avg = init_avg;
            _max_increment = max_increment;
        }

        R get_average() const {
            return static_cast<R>(_sum / static_cast<V>(_window_size));
        }

        float get_init_average() const {
            return _init_avg;
        }

        auto get_window_size() const {
            return _window_size;
        }

        // DEBUG
        auto &get_buffer() {
            return _buf;
        }
    };


    template <typename V, size_t FilterBufferSize = DefaultFilterBufferSize>
    class SignalMasseuse : public ValueClamp<V> {
       public:
        using FilterType = IFilter<V, V, FilterBufferSize>;

       private:
        FilterType *_pre_filter = nullptr;
        FilterType *_post_filter = nullptr;

       public:
        SignalMasseuse(V limit_min, V limit_max) :
            ValueClamp<V>(limit_min, limit_max) {}

        SignalMasseuse(V limit_min, V limit_max, V mapping_min, V mapping_max) :
            ValueClamp<V>(limit_min, limit_max, mapping_min, mapping_max) {}

        SignalMasseuse() = default;

        void set_pre_filter(FilterType *ftr) {
            this->_pre_filter = ftr;
        }

        void set_post_filter(FilterType *ftr) {
            this->_post_filter = ftr;
        }

        FilterType *get_pre_filter() {
            return this->_pre_filter;
        }

        FilterType *get_post_filter() {
            return this->_post_filter;
        }

        V count(V val) {
            if (_pre_filter != nullptr)
                val = _pre_filter->feed(val);

            val = static_cast<ValueClamp<V> *>(this)->count(val);

            if (_post_filter != nullptr)
                val = _post_filter->feed(val);

            return val;
        }
    };


    /**
     * @brief 表示控制器输出与被控量变化方向是否一致。
     *
     * direct: 方向一致，输出增大时被控量也增大；
     * reverse：方向相反
     *
     */
    enum class pid_direction {
        direct,
        reverse,
    };


    enum class p_mode : uint8_t {
        on_error,
        on_measure,
    };


    enum class d_mode : uint8_t {
        on_error,
        on_measure,
    };


    /**
     * @brief PID 控制器的细节策略调整
     *
     */
    // enum class pid_strategy {
    //     i_stop_initially,   // 初始状态或误差较大时停止积分
    //     dead_zone,  // 误差小于死区时关闭PID, 保持输出不变
    //     dead_zone_i, // 误差小于死区时，关闭P 和D，只积分
    // };


    class PidCountCore {
       public:
        using ClampType = SignalMasseuse<float, 32>;
        using FilterType = ClampType::FilterType;

       private:
        float _original_kp;
        float _original_ki, _original_kd;  // 输入的PID 参数原始值
        float _timing_ki, _timing_kd;      // 用采样时间计算后的实际KI，KD 参数，KP 和时间无关

        float _last_feedback;
        float _last_error;
        float _sample_time;

        float _p_term, _i_term, _d_term;  // 用来显示三项的值

        pid_direction _direction = pid_direction::direct;

        p_mode _pm = p_mode::on_error;
        d_mode _dm = d_mode::on_measure;  // 默认为测速反馈模式

        bool _i_clamp_by_output_limit = false;
        bool _i_clamp_by_integral_limit = false;

        // i_clamp _im = i_clamp::off;   // 默认不对积分限幅
        float _integral_min;
        float _integral_max;

        // 在反馈和输出端分别附加一个环节用于限幅、映射、滤波
        ClampType _feedback_clamp;
        ClampType _output_clamp;


       public:
        /**
         * @brief Construct a new Pid Count Core object
         *
         * @param kp
         * @param ki
         * @param kd
         * @param sample_time   采样或计算周期，以秒为单位
         */
        PidCountCore(float kp, float ki, float kd, float sample_time) :
            _original_kp(kp), _original_ki(ki), _original_kd(kd), _sample_time(sample_time) {}

        /**
         * @brief Construct a new Pid Count Core object
         *
         * 默认 KP == KI == KD == 0，采样周期0.1s（100ms）
         *
         */
        PidCountCore() :
            PidCountCore(0, 0, 0, 0.1f) {}

        void tune(float kp, float ki, float kd) {
            assert(!(kp < 0 || ki < 0 || kd < 0), "KP, KI, KD should be greater than zero");

            _original_kp = kp;
            _original_ki = ki;
            _original_kd = kd;

            _timing_ki = k_integral(ki);
            _timing_kd = k_differential(kd);
        }

        void tune(float kp, float ki, float kd, float sample_time) {
            _sample_time = sample_time;
            this->tune(kp, ki, kd);
        }

        /**
         * @brief 对误差的积分计算可以由ki 代劳。
         *
         * @param k
         * @return float
         */
        float k_integral(float k) const {
            return k * _sample_time;
        }

        /**
         * @brief 对误差的微分计算可以由kd 代劳。
         *
         * 原理是：kd * (dE / dT) == (kd / dT) * dE，从而只需在调参时做一次除法。
         * 外部调用者可以用这个函数计算其他参数，从而与PID 对象“时间同步”
         *
         * @param k
         * @return float
         */
        float k_differential(float k) const {
            return k / _sample_time;
        }

        float count(float input, float feedback) {
            /* 参考代码: https://github.com/br3ttb/Arduino-PID-Library, License: MIT  */

            feedback = _feedback_clamp.count(feedback);
            float d_feedback = feedback - _last_feedback;
            float error = input - feedback;

            if (_direction == pid_direction::reverse) {  // 取反误差，让反向控制规律变回正向
                d_feedback = 0 - d_feedback;
                error = 0 - error;
            }

            _last_error = error;
            _last_feedback = feedback;

            if (_pm == p_mode::on_measure) {
                _p_term = 0 - _original_kp * d_feedback;
            }
            else {
                _p_term = _original_kp * error;
            }

            if (_dm == d_mode::on_measure) {
                _d_term = 0 - _original_kd * d_feedback;
            }
            else {
                _d_term = _original_kd * error;
            }

            _i_term += _timing_ki * error;

            float out = _p_term + _d_term;

            if (_i_clamp_by_integral_limit) {
                _i_term = value_constrain(_i_term, _integral_min, _integral_max);
            }

            out += _i_term;

            if (_i_clamp_by_output_limit) {
                float out_min = _output_clamp.get_limit_min();
                float out_max = _output_clamp.get_limit_max();

                if (out > out_max) {
                    _i_term -= out - out_max;
                    out = out_max;
                }
                else if (out < out_min) {
                    _i_term += out_min - out;
                    out = out_min;
                }
            }

            return _output_clamp.count(out);
        }

        void set_sample_time(float sample_time) {
            _sample_time = sample_time;
            this->tune(_original_kp, _original_ki, _original_kd);
        }

        float get_sample_time() const {
            return _sample_time;
        }

        float get_kp() const {
            return _original_kp;
        }

        float get_ki() const {
            return _original_ki;
        }

        float get_kd() const {
            return _original_kd;
        }

        float get_p_term() const {
            return _p_term;
        }

        float get_i_term() const {
            return _i_term;
        }

        float get_d_term() const {
            return _d_term;
        }

        void set_d_mode(d_mode dm) {
            _dm = dm;
        }

        d_mode get_d_mode() const {
            return _dm;
        }

        void set_p_mode(p_mode pm) {
            _pm = pm;
        }

        p_mode get_p_mode() const {
            return _pm;
        }

        void set_direction(pid_direction d) {
            _direction = d;
        }

        pid_direction get_direction() const {
            return _direction;
        }

        void enable_i_clamp_by_integral_limit(float integral_min, float integral_max) {
            _integral_min = integral_min;
            _integral_max = integral_max;
            _i_clamp_by_integral_limit = true;
        }

        void disable_i_clamp_by_integral_limit() {
            _i_clamp_by_integral_limit = false;
        }

        void enable_i_clamp_by_output_limit(float out_min, float out_max) {
            // enable_i_clamp_by_output_limit();
            _output_clamp.set_limit(out_min, out_max);
            _i_clamp_by_output_limit = true;
        }

        void disable_i_clamp_by_output_limit() {
            _i_clamp_by_integral_limit = false;
        }

        ClampType &get_feedback_clamp() {
            return _feedback_clamp;
        }

        ClampType &get_output_clamp() {
            return _output_clamp;
        }

        const ClampType &get_feedback_clamp() const {
            return _feedback_clamp;
        }

        const ClampType &get_output_clamp() const {
            return _output_clamp;
        }

        void enable_feedback_limit(float min, float max) {
            _feedback_clamp.set_limit(min, max);
            _feedback_clamp.enable_limit();
        }

        void enable_output_limit(float min, float max) {
            _output_clamp.set_limit(min, max);
            _feedback_clamp.enable_limit();
        }

        void disable_feedback_limit() {
            _feedback_clamp.disable_limit();
        }

        void disable_output_limit() {
            _output_clamp.disable_limit();
        }

        void enable_feedback_mapping(float min, float max) {
            _feedback_clamp.set_mapping(min, max);
            _feedback_clamp.enable_mapping();
        }

        void enable_output_mapping(float min, float max) {
            _output_clamp.set_mapping(min, max);
            _feedback_clamp.enable_mapping();
        }

        void disable_feedback_mapping() {
            _feedback_clamp.disable_mapping();
        }

        void disable_output_mapping() {
            _output_clamp.disable_mapping();
        }

        void enable_feedback_pre_filter(FilterType *ftr) {
            _feedback_clamp.set_pre_filter(ftr);
        }

        void enable_feedback_post_filter(FilterType *ftr) {
            _feedback_clamp.set_post_filter(ftr);
        }

        void disable_feedback_pre_filter() {
            _feedback_clamp.set_pre_filter(nullptr);
        }

        void disable_feedback_post_filter() {
            _feedback_clamp.set_post_filter(nullptr);
        }

        void enable_output_pre_filter(FilterType *ftr) {
            _output_clamp.set_pre_filter(ftr);
        }

        void enable_output_post_filter(FilterType *ftr) {
            _output_clamp.set_post_filter(ftr);
        }

        void disable_output_pre_filter() {
            _output_clamp.set_pre_filter(nullptr);
        }

        void disable_output_post_filter() {
            _output_clamp.set_post_filter(nullptr);
        }

        void reset() {
            _i_term = 0;
            _last_error = 0;
            _last_feedback = 0;
        }

        void stop() {
            // ;  // do noting
        }

        void resume(float current_input, float current_feedback, float current_output) {
            // 令误差归零，比例和微分项归零，然后调整积分项，使输出等于设置的输出值
            // TODO: 有点笨拙，而且没有考虑滤波器
            float current_error = current_input - current_feedback;
            _last_error = current_error;
            _last_feedback = current_feedback;
            float out = count(current_input, current_feedback);
            _i_term += current_output - out;
        }
    };


    /**
     * @brief PID 位置 - 速度双闭环控温，加热和制冷双输出
     *
     */
    class BicycleThermalPid {
       public:
        using GcIndexType = uint8_t;
        static constexpr GcIndexType PidStageCount = 2;
        using FilterType = PidCountCore::FilterType;

       private:
        PidCountCore _gc[PidStageCount];  // 两个PID 串联

        PidCountCore &_gc0 = _gc[0];
        PidCountCore &_gc1 = _gc[1];

        float _last_main_feedback = 0;
        float _original_kv = 1;
        float _timing_kv = 0;
        float _gc0_out = 0;
        // float _sample_time;  // TODO: 可能没用

        unsigned int _gc1_freq_times = 1;  // gc1 采样频率对gc0 的倍率，若倍率大于1，则gc1 计算多次后，gc0 才计算一次
        unsigned int _gc0_wait_counter = 0;

       public:
        void tune_gc0(float kp, float ki, float kd) {
            _gc0.tune(kp, ki, kd);
        }

        void tune_gc1(float kp, float ki, float kd, float kv = 1.0f) {
            _gc1.tune(kp, ki, kd);
            _timing_kv = _gc1.k_differential(kv);
            _original_kv = kv;
        }

        void set_sample_time(float gc1_sample_time, unsigned int gc1_freq_times) {
            // _gc1_sample_time = gc1_sample_time;
            float gc0_sample_time = gc1_sample_time * static_cast<float>(gc1_freq_times);
            _gc1_freq_times = gc1_freq_times;
            _gc0.set_sample_time(gc0_sample_time);
            _gc1.set_sample_time(gc1_sample_time);
            _timing_kv = _gc1.k_differential(_original_kv);
        }

        float forward_pass(float set_point, float gc0_feedback, float gc1_feedback) {
            _gc0_wait_counter++;
            if (_gc0_wait_counter == _gc1_freq_times) {
                _gc0_out = _gc0.count(set_point, gc0_feedback);
                _gc0_wait_counter = 0;
            }

            float gc1_out = _gc1.count(_gc0_out, gc1_feedback);
            return gc1_out;
        }

        float step_v_feedback(float set_point, float main_feedback) {
            float v_feedback = _timing_kv * (main_feedback - _last_main_feedback);  // 反馈微分
            _last_main_feedback = main_feedback;

            return forward_pass(set_point, main_feedback, v_feedback);
        }

        float step_separate(float set_point, float main_feedback, float inner_feedback) {
            return forward_pass(set_point, main_feedback, inner_feedback);
        }

        const PidCountCore &get_gc0() const {
            return _gc0;
        }

        const PidCountCore &get_gc1() const {
            return _gc1;
        }

       private:
        PidCountCore &_get_gc_by_index(GcIndexType which) {
            // assert(which < PidStageCount, "Index Out Of Bound.");
            return _gc[which];
        }

        template <GcIndexType Which>
        constexpr PidCountCore &_get_gc_by_index() {
            static_assert(Which < PidStageCount, "Index Out Of Bound.");
            return _gc[Which];
        }

        void reset() {
            for (int i = 0; i < PidStageCount; ++i) {
                _gc[i].reset();
            }
            _gc0_out = 0;
            _gc0_wait_counter = 0;
            _last_main_feedback = 0;
        }

       public:
        template <GcIndexType Which>
        void set_gc_pd_mode(p_mode pm, d_mode dm) {
            _get_gc_by_index<Which>().set_d_mode(dm);
            _get_gc_by_index<Which>().set_d_mode(pm);
        }

        template <GcIndexType Which>
        void enable_gc_integral_clamp(float min, float max) {
            _get_gc_by_index<Which>().enable_i_clamp_by_integral_limit(min, max);
        }

        template <GcIndexType Which>
        void disable_gc_integral_clamp() {
            _get_gc_by_index<Which>().disable_i_clamp_by_integral_limit();
        }

        template <GcIndexType Which>
        void enable_gc_integral_clamp_by_output_limit(float min, float max) {
            _get_gc_by_index<Which>().enable_i_clamp_by_output_limit(min, max);
        }

        template <GcIndexType Which>
        void disable_gc_integral_clamp_by_output_limit() {
            _get_gc_by_index<Which>().disable_i_clamp_by_output_limit();
        }

        template <GcIndexType Which>
        void enable_gc_output_limit(float min, float max) {
            _get_gc_by_index<Which>().enable_output_limit(min, max);
        }

        template <GcIndexType Which>
        void enable_gc_feedback_limit(float min, float max) {
            _get_gc_by_index<Which>().enable_feedback_limit(min, max);
        }

        template <GcIndexType Which>
        void disable_gc_output_limit() {
            _get_gc_by_index<Which>().disable_output_limit();
        }

        template <GcIndexType Which>
        void disable_gc_feedback_limit() {
            _get_gc_by_index<Which>().disable_feedback_limit();
        }

        template <GcIndexType Which>
        void enable_gc_output_mapping(float min, float max) {
            _get_gc_by_index<Which>().enable_output_mapping(min, max);
        }

        template <GcIndexType Which>
        void enable_gc_feedback_mapping(float min, float max) {
            _get_gc_by_index<Which>().enable_feedback_mapping(min, max);
        }

        template <GcIndexType Which>
        void disable_gc_output_mapping() {
            _get_gc_by_index<Which>().disable_output_mapping();
        }

        template <GcIndexType Which>
        void disable_gc_feedback_mapping() {
            _get_gc_by_index<Which>().disable_feedback_mapping();
        }

        template <GcIndexType Which>
        void set_gc_output_filter(FilterType *pre_filter, FilterType *post_filter) {
            _get_gc_by_index<Which>().enable_output_pre_filter(pre_filter);
            _get_gc_by_index<Which>().enable_output_post_filter(post_filter);
        }

        template <GcIndexType Which>
        void set_gc_feedback_filter(FilterType *pre_filter, FilterType *post_filter) {
            _get_gc_by_index<Which>().enable_feedback_pre_filter(pre_filter);
            _get_gc_by_index<Which>().enable_feedback_post_filter(post_filter);
        }
    };


    struct HeaterTempPoint {
        float temp;  // 温度点，每个温度点对应一组温升速度和散热功率，低于这个温度点但高于前一个温度时，
        // 采用该点对应的参数
        float temp_increment;      // 加热器温升速度，单位是℃/W·sec，即对应一瓦功率的每秒温升
        float dissipation_factor;  // 散热速度，单位是W/℃，即热阻的倒数，以室温25 ℃做参考。
        // 在室温下散热功率为0，输入功率为0 时，加热器温度保持不变
    };


    constexpr bool operator<(const HeaterTempPoint &left, const HeaterTempPoint &right) {
        return left.temp < right.temp;
    }


    template <size_t MaxPointCount = 10, size_t MaxDelayBufferSize = 50, size_t ParamCornerRounding = 10>
    class HeatSim {
       public:
        using DelayBufferType = MovingAverageFilter<float, float, MaxDelayBufferSize>;
        using ParamSmoothBufferType = MovingAverageFilter<float, float, ParamCornerRounding>;
        using PointPtrListType = data_basic::PointerList<HeaterTempPoint, MaxPointCount>;

       private:
        DelayBufferType _power_delay;
        DelayBufferType _sensor_delay;
        DelayBufferType _temp_delay;  // 加热器温度如果没有滑动滤波，散热功率变化时温度会突变，导致散热功率在参数分段点附近震荡

        ParamSmoothBufferType _temp_increment_rounding;
        ParamSmoothBufferType _dissipation_factor_rounding;

        PointPtrListType _point_ptr_list;
        HeaterTempPoint _point_list[MaxPointCount];

        float _temp_heater = 0;

        float _current_heat_increment = 0;
        float _current_dissipation_factor = 0;

        float _sample_time = 0.1f;

       public:
        HeatSim(size_t power_delay_count, size_t temp_delay_count, size_t sensor_delay_count) :
            _power_delay{0, power_delay_count},
            _sensor_delay{0, sensor_delay_count},
            _temp_delay{0, temp_delay_count} {}

        void add_point(float temp, float increment_per_watt_second, float dissipation_power_per_celsius) {
            auto size = _point_ptr_list.size();
            auto &p = _point_list[size];

            p.temp = temp;
            p.temp_increment = increment_per_watt_second;
            p.dissipation_factor = dissipation_power_per_celsius;

            _point_ptr_list.push_back(&p);
            _point_ptr_list.sort();
        }

        /**
         * @brief 获取指定温度下生效的温度参数
         *
         * @param temp
         * @return const HeaterTempPoint &若没有添加温度点，返回参数全为零
         */
        const HeaterTempPoint &get_related_point(float temp) const {
            static constexpr HeaterTempPoint zero_point = {0, 0, 0};

            if (_point_ptr_list.size() == 0) {
                return zero_point;
            }
            else {
                for (const auto &ptr_to_point : _point_ptr_list) {
                    const HeaterTempPoint &p = *ptr_to_point;
                    float t = p.temp;
                    if (t > temp) {
                        return p;
                    }
                }

                // 表里不存在温度高于temp 的点时，使用最后一个点的参数
                return _point_ptr_list.back().to_object();
            }
        }

        /**
         * @brief 用滑动滤波使参数转折处变得平滑
         *
         * @param p
         */
        void smooth_point(const HeaterTempPoint &p) {
            // TODO:
            // 如果滑动滤波的初始值为0，表示滤波器刚刚初始化，
            // 此时用添加的第一个点的数据初始化滤波器，而不是将数据喂进去

            _current_heat_increment = _temp_increment_rounding.feed(p.temp_increment);
            _current_dissipation_factor = _dissipation_factor_rounding.feed(p.dissipation_factor);
        }

        /**
         * @brief 执行一次模拟
         *
         * 模拟的初始温度为0。
         *
         * @return float 传感器输出的温度值
         */
        float sim(float input_power) {
            float power = _power_delay.feed(input_power);

            const HeaterTempPoint &p = get_related_point(_temp_heater);
            smooth_point(p);

            float dissipation = _current_dissipation_factor * (_temp_heater - 25);  // 用加热器相对室温25 的温差计算散热功率

            float joule = (power - dissipation) * _sample_time;

            if (std::abs(joule) < 1e-5) {
                joule = 0;
            }

            _temp_heater += _current_heat_increment * joule;
            _temp_heater = _temp_delay.feed(_temp_heater);

            // DEBUG
            // std::cout << "Temp: " << _temp_heater << "; Dissipation: " << dissipation << "; Power: " << power << "; Joule: " << joule << std::endl;

            return _sensor_delay.feed(_temp_heater);
        }

        void set_sample_time(float sec) {
            _sample_time = sec;
        }

        void set_power_delay(float init_val, size_t count) {
            _power_delay.init(init_val, count);
            _power_delay.reset();
        }

        void set_sensor_delay(float init_val, size_t count) {
            _sensor_delay.init(init_val, count);
            _sensor_delay.reset();
        }

        void set_temp_delay(float init_val, size_t count) {
            _temp_delay.init(init_val, count);
            _temp_delay.reset();
        }

        float get_heater_temp() const {
            return _temp_heater;
        }

        void set_heater_temp(float val) {
            _temp_heater = val;

            _temp_delay.init(val, _temp_delay.get_window_size());
            _temp_delay.reset();

            _sensor_delay.init(val, _sensor_delay.get_window_size());
            _sensor_delay.reset();
        }

        float get_dissipation_factor() const {
            return _current_dissipation_factor;
        }

        float get_temp_increment() const {
            return _current_heat_increment;
        }

        // DEBUG
        const auto &get_point_list() const {
            return _point_list;
        }

        // DEBUG
        const auto &get_point_ptr_list() const {
            return _point_ptr_list;
        }

        // DEBUG
        float get_delayed_power() const {
            return _power_delay.get_average();
        }
    };

}  // namespace lipid
